#include "step_ctrl.h"
STEP_MOTOR motor1;
STEP_MOTOR motor2;

STEP_MOTOR *arr[2] = {&motor1, &motor2};

// 电机结构体初始化
void step_struct_init(STEP_MOTOR *motor, uint8_t addr_set, uint8_t dir_set, uint16_t vel_set, uint8_t acc_set, uint32_t clk_set, bool raF_set, bool snF_set)
{
    motor->acc = acc_set;
    motor->addr = addr_set;
    motor->clk = clk_set;
    motor->dir = dir_set;
    motor->raF = raF_set;
    motor->snF = snF_set;
    motor->vel = vel_set;
}
// 电机结构体去初始化
void step_struct_deinit(STEP_MOTOR *motor)
{
    motor->acc = 0;
    motor->addr = 0;
    motor->clk = 0;
    motor->dir = 0;
    motor->raF = 0;
    motor->snF = 0;
    motor->vel = 0;
}
// 设置电机结构体地址参数
void step_struct_setaddr(STEP_MOTOR *motor, uint8_t addr_set)
{
    motor->addr = addr_set;
}
// 设置电机结构体方向参数

void step_struct_setdir(STEP_MOTOR *motor, uint8_t dir_set)
{
    motor->dir = dir_set;
}
// 设置电机结构体速度参数

void step_struct_setvel(STEP_MOTOR *motor, uint16_t vel_set)
{
    motor->vel = vel_set;
}
// 设置电机结构体加速度参数

void step_struct_setacc(STEP_MOTOR *motor, uint8_t acc_set)
{
    motor->acc = acc_set;
}
// 设置电机结构体脉冲参数

void step_struct_setclk(STEP_MOTOR *motor, uint32_t clk_set)
{
    motor->clk = clk_set;
}
// 设置电机结构体相对运动参数

void step_struct_setraF(STEP_MOTOR *motor, bool raF_set)
{
    motor->raF = raF_set;
}
// 设置电机结构体多机同步参数

void step_struct_setsnF(STEP_MOTOR *motor, bool snF_set)
{
    motor->snF = snF_set;
}
void step_struct_setangle(STEP_MOTOR *motor, float angle_set)
{
    motor->angle = angle_set;
}

void step_struct_settemp(STEP_MOTOR *motor, uint32_t temp)
{
    motor->temp = temp;
}
/**
 * @brief    多电机位置模式
 * @param    addr：电机地址
 * @param    dir ：方向        ，0为CW，其余值为CCW
 * @param    vel ：速度(RPM)   ，范围0 - 5000RPM
 * @param    acc ：加速度      ，范围0 - 255，注意：0是直接启动
 * @param    clk ：脉冲数      ，范围0- (2^32 - 1)个
 * @param    raF ：相位/绝对标志，false为相对运动，true为绝对值运动
 * @param    snF ：多机同步标志 ，false为不启用，true为启用
 * @param    num ：电机数量
 * @retval   地址 + 功能码 + 命令状态 + 校验字节
 */
void multiple_step_syn_position_ctrl(STEP_MOTOR *motor[], uint8_t num)
{

    uint8_t i = 0;
    for (i = 0; i < num; i++)
    {
        Emm_V5_Pos_Control(motor[i]->addr, motor[i]->dir, motor[i]->vel, motor[i]->acc, motor[i]->clk, motor[i]->raF, motor[i]->snF); // 多机同步标志位置1
        delay_ms(10);                                                                                                                // 每条命令后面延时10毫秒，防止粘包
    }
    Emm_V5_Synchronous_motion(0); // 广播地址0触发多机同步开始运动
    delay_ms(10);                // 每条命令后面延时10毫秒，防止粘包
#ifdef USE_CAN
    while (can.rxData[0] != 0xFD || can.rxData[1] != 0x9F)
        ;
    can.rxFrameFlag = false; // 等待返回到位命令
#endif
#ifdef USE_USART
    while (rxCmd[1] != 0xFD || rxCmd[2] != 0x9F)
        ;
    rxFrameFlag = false;
#endif
}
// 读取电机实时位置
// float step_read_real_position(STEP_MOTOR *motor)
// {
//     // 定义实时位置全局变量
//     float pos = 0.0f, Motor_Cur_Pos = 0.0f;
//     Emm_V5_Read_Sys_Params(motor->addr, S_CPOS);
// #ifdef DEBUG
//     while (can.rxFrameFlag == false)
//         ;
//     can.rxFrameFlag = false; // 等待返回命令
//     if (can.rxData[0] == 0x36 && can.CAN_RxMsg.DLC == 7)
//     {
//         // 拼接成uint32_t类型
//         pos = (uint32_t)(((uint32_t)can.rxData[2] << 24) |
//                          ((uint32_t)can.rxData[3] << 16) |
//                          ((uint32_t)can.rxData[4] << 8) |
//                          ((uint32_t)can.rxData[5] << 0));
//         // 转换成角度
//         Motor_Cur_Pos = (float)pos * 360.0f / 65536.0f;
//         // 符号
//         if (can.rxData[1])
//         {
//             Motor_Cur_Pos = -Motor_Cur_Pos;
//         }
//     }
//     return Motor_Cur_Pos;
// #endif
// #ifdef USE_USART
//     while (rxFrameFlag == false)
//         ;
//     rxFrameFlag = false;
//     if (rxCmd[0] == 1 && rxCmd[1] == 0x36 && rxCount == 8)
//     {
//         // 拼接成uint32_t类型
//         pos = (uint32_t)(((uint32_t)rxCmd[3] << 24) |
//                          ((uint32_t)rxCmd[4] << 16) |
//                          ((uint32_t)rxCmd[5] << 8) |
//                          ((uint32_t)rxCmd[6] << 0));

//         // 转换成角度
//         Motor_Cur_Pos = (float)pos * 360.0f / 65536.0f;

//         // 符号
//         if (rxCmd[2])
//         {
//             Motor_Cur_Pos = -Motor_Cur_Pos;
//         }
//     }
//     return Motor_Cur_Pos;
// #endif
// }
// 单电机位置模式
/**
 * @brief    位置模式
 * @param    addr：电机地址
 * @param    dir ：方向        ，0为CW，其余值为CCW
 * @param    vel ：速度(RPM)   ，范围0 - 5000RPM
 * @param    acc ：加速度      ，范围0 - 255，注意：0是直接启动
 * @param    clk ：脉冲数      ，范围0- (2^32 - 1)个
 * @param    raF ：相位/绝对标志，false为相对运动，true为绝对值运动
 * @param    snF ：多机同步标志 ，false为不启用，true为启用
 * @retval   地址 + 功能码 + 命令状态 + 校验字节
 */
void step_position_ctrl(STEP_MOTOR *motor)
{
    Emm_V5_Pos_Control(motor->addr, motor->dir, motor->vel, motor->acc, motor->clk, motor->raF, motor->snF);
#ifdef USE_CAN
    while (can.rxFrameFlag == false)
        ;
    can.rxFrameFlag = false; // 等待返回命令
#endif
#ifdef USE_USART
    while (rxFrameFlag == false)
        ;
    rxFrameFlag = false;
#endif
}

// 单电机速度模式设置
/**
 * @brief    速度模式
 * @param    addr：电机地址
 * @param    dir ：方向       ，0为CW，其余值为CCW
 * @param    vel ：速度       ，范围0 - 5000RPM
 * @param    acc ：加速度     ，范围0 - 255，注意：0是直接启动
 * @param    snF ：多机同步标志，false为不启用，true为启用
 * @retval   地址 + 功能码 + 命令状态 + 校验字节
 */
void step_vel_ctrl(STEP_MOTOR *motor)
{
    Emm_V5_Vel_Control(motor->addr, motor->dir, motor->vel, motor->acc, motor->snF);
#ifdef USE_CAN
    while (can.rxFrameFlag == false)
        ;
    can.rxFrameFlag = false; // 等待返回命令
#endif
#ifdef USE_USART
    while (rxFrameFlag == false)
        ;
    rxFrameFlag = false;
#endif
}

// 多电机同步速度模式设置
/**
 * @brief    速度模式
 * @param    addr：电机地址
 * @param    dir ：方向       ，0为CW，其余值为CCW
 * @param    vel ：速度       ，范围0 - 5000RPM
 * @param    acc ：加速度     ，范围0 - 255，注意：0是直接启动
 * @param    snF ：多机同步标志，false为不启用，true为启用
 * @param    num : 电机数量
 * @retval   地址 + 功能码 + 命令状态 + 校验字节
 */
void multiple_step_syn_vel_ctrl(STEP_MOTOR *motor[], uint8_t num) // 此函数最后命令返回需要验证
{
    uint8_t i = 0;
    for (i = 0; i < num; i++)
    {
        Emm_V5_Vel_Control(motor[i]->addr, motor[i]->dir, motor[i]->vel, motor[i]->acc, motor[i]->snF); // 多机同步标志位置1
        delay_ms(10);                                                                                  // 每条命令后面延时10毫秒，防止粘包
    }
    Emm_V5_Synchronous_motion(0); // 广播地址0触发多机同步开始运动
    delay_ms(10);                // 每条命令后面延时10毫秒，防止粘包
#ifdef USE_CAN
    while (can.rxData[0] != 0xFD || can.rxData[1] != 0x02)
        ;
    can.rxFrameFlag = false; // 等待返回到位命令
#endif
#ifdef USE_USART
    while (rxCmd[1] != 0xFD || rxCmd[2] != 0x9F)
        ;
    rxFrameFlag = false;
#endif
}
// 读取指定电机实时转速
// float step_read_real_vel(STEP_MOTOR *motor)
// {
//     float vel = 0.0f, Motor_Vel = 0.0f;
//     delay_ms(1000); // 延时1秒，等待转速到达
//     Emm_V5_Read_Sys_Params(motor->addr, S_VEL);
// #ifdef USE_CAN
//     while (can.rxFrameFlag == false)
//         ;
//     can.rxFrameFlag = false; // 等待返回命令

//     if (can.rxData[0] == 0x35 && can.CAN_RxMsg.DLC == 5)
//     {
//         // 拼接成uint16_t类型数据
//         vel = (uint16_t)(((uint16_t)can.rxData[2] << 8) |
//                          ((uint16_t)can.rxData[3] << 0));

//         // 实时转速
//         Motor_Vel = vel;
//         // 符号
//         if (can.rxData[1])
//         {
//             Motor_Vel = -Motor_Vel;
//         }
//     }
//     return Motor_Vel;
// #endif
// #ifdef USE_USART
//     while (rxFrameFlag == false)
//         ;
//     rxFrameFlag = false;
//     if (rxCmd[0] == 1 && rxCmd[1] == 0x35 && rxCount == 6)
//     {
//         // 拼接成uint16_t类型数据
//         vel = (uint16_t)(((uint16_t)rxCmd[3] << 8) |
//                          ((uint16_t)rxCmd[4] << 0));

//         // 实时转速
//         Motor_Vel = vel;

//         // 符号
//         if (rxCmd[2])
//         {
//             Motor_Vel = -Motor_Vel;
//         }
//     }
//     return Motor_Vel;
// #endif
// }
// 单电机回零命令，默认为单圈就近回零
void step_backzero(STEP_MOTOR *motor)
{
    delay_ms(1000); // 等待位置到达
    Emm_V5_Origin_Trigger_Return(motor->addr, 0, false);
#ifdef USE_CAN
    while (can.rxFrameFlag == false)
        ;
    can.rxFrameFlag = false; // 等待返回命令
#endif
#ifdef USE_USART
    while (rxFrameFlag == false)
        ;
    rxFrameFlag = false;
#endif
}
// 设置回零零点，不存储，断电即没
void step_zero_set_nosave(STEP_MOTOR *motor)
{
    delay_ms(2000);                     // 等待闭环初始化
    Emm_V5_Origin_Set_O(motor->addr, 0); // 设置当前位置为单圈回零的零点位置，不储存
#ifdef USE_CAN
    while (can.rxFrameFlag == false)
        ;
    can.rxFrameFlag = false;
#endif // DEBUG
#ifdef USE_USART
    while (rxFrameFlag == false)
        ;
    rxFrameFlag = false; // 等待命令返回
#endif                   // DEBUG
}

// 设置回零零点，存储
void step_zero_set_save(STEP_MOTOR *motor)
{
    delay_ms(2000);                     // 等待闭环初始化
    Emm_V5_Origin_Set_O(motor->addr, 1); // 设置当前位置为单圈回零的零点位置，存储
#ifdef USE_CAN
    while (can.rxFrameFlag == false)
        ;
    can.rxFrameFlag = false;
#endif // DEBUG
#ifdef USE_USART
    while (rxFrameFlag == false)
        ;
    rxFrameFlag = false; // 等待命令返回
#endif                   // DEBUG
}

void step_angle_ctrl(STEP_MOTOR *motor) // 3200脉冲
{
    // 方向判断
    if ((motor->angle) >= 0)
    {
        motor->dir = 0;
    }
    if ((motor->angle) < 0)
    {
        motor->dir = 1;
        motor->angle = -(motor->angle);
    }
    motor->clk = (motor->angle) / 360.0f * (motor->temp);
    motor->raF = 1;
    step_position_ctrl(motor);
    motor->raF = 0; // 默认为相对运动
}

void step_angle_ctrl_syn(STEP_MOTOR *motor) // 3200脉冲
{
    // 方向判断
    if ((motor->angle) >= 0)
    {
        motor->dir = 0;
    }
    if ((motor->angle) < 0)
    {
        motor->dir = 1;
        motor->angle = -(motor->angle);
    }
    motor->clk = (motor->angle) / 360.0f * (motor->temp);
    motor->raF = 1;
    Emm_V5_Pos_Control(motor->addr, motor->dir, motor->vel, motor->acc, motor->clk, motor->raF, motor->snF);
    motor->raF = 0; // 默认为相对运动
}

void multiple_step_syn_angle_ctrl(STEP_MOTOR *motor[], uint8_t num)
{

    uint8_t i = 0;
    for (i = 0; i < num; i++)
    {
        step_angle_ctrl_syn(motor[i]); // 多机同步标志位置1
        delay_ms(10);                 // 每条命令后面延时10毫秒，防止粘包
    }
    Emm_V5_Synchronous_motion(0); // 广播地址0触发多机同步开始运动
    delay_ms(10);                // 每条命令后面延时10毫秒，防止粘包
#ifdef USE_CAN
    while (can.rxData[0] != 0xFD || can.rxData[1] != 0x9F)
        ;
    can.rxFrameFlag = false; // 等待返回到位命令
#endif
#ifdef USE_USART
    while (rxCmd[1] != 0xFD || rxCmd[2] != 0x9F)
        ;
    rxFrameFlag = false;
#endif
}